/**
* @author: Jinghui Wang
*/

#ifndef POINTCLOUD_TO_GRID_NODE_HPP
#define POINTCLOUD_TO_GRID_NODE_HPP

#include <limits>
#include <string>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/passthrough.h>

namespace p2g {

class PointCloudToGridNode : public rclcpp::Node {
public:
    PointCloudToGridNode(const rclcpp::NodeOptions & options);

private:
    void cloudCallBack(const sensor_msgs::msg::PointCloud2::SharedPtr msg);

    void timerCallBack();

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr pub_;
    nav_msgs::msg::OccupancyGrid grid_;

    // For a certain update frequency
    rclcpp::TimerBase::SharedPtr timer_;
    sensor_msgs::msg::PointCloud2::SharedPtr latest_cloud_mag_;

    // ROS Parameters
    std::string input_topic_;
    std::string output_topic_;
    std::string output_frame_;

    double min_height_;
    double max_height_;
    double map_resolution_;
    double map_origin_x_;
    double map_origin_y_;
    double update_period_;
};

} // namespace p2g


#endif //POINTCLOUD_TO_GRID_NODE_HPP
